﻿using System;
using System.Collections.Generic;
using OpenTK;

namespace Chimp.Core.Physics
{
    public class ChimpVP : ChimpBase
    {
        public Vector2 Gravity = new Vector2(0, 45.98f);
        public List<ChimpVPhysicsNode> Nodes = new List<ChimpVPhysicsNode>();

        public ChimpVPhysicsBody CreateChain(float x, float y, float xi, float yi, int num, float mass)
        {
            var b = new ChimpVPhysicsBody();
            ChimpVPhysicsNode ln = null;
            for (int i = 0; i < num/2; i++)
            {
                ChimpVPhysicsNode n = GenNode(ref x, ref y, xi, yi, mass);
                if (ln != null)
                {
                    ln.LinkTo(n);
                }
                ln = n;
                b.Nodes.Add(n);
            }
            return b;
        }

        private static ChimpVPhysicsNode GenNode(ref float x, ref float y, float xi, float yi, float mass)
        {
            var n = new ChimpVPhysicsNode(x, y, mass);
            x += xi;
            y += yi;
            return n;
        }

        public ChimpVPhysicsBody CreateBox(float x, float y, float w, float h, float mass)
        {
            var body = new ChimpVPhysicsBody();
            ChimpVPhysicsNode n1, n2, n3, n4;

            n1 = new ChimpVPhysicsNode(x - w, y - w, mass);
            n2 = new ChimpVPhysicsNode(x + w, y - h, mass);
            n3 = new ChimpVPhysicsNode(x + w, y + h, mass);
            n4 = new ChimpVPhysicsNode(x - w, y + h, mass);

            body.Nodes.Add(n1);
            body.Nodes.Add(n2);
            body.Nodes.Add(n3);
            body.Nodes.Add(n4);

            n1.LinkTo(n2);
            n2.LinkTo(n4);

            n2.LinkTo(n3);

            n4.LinkTo(n3);

            n1.LinkTo(n3);
            n2.LinkTo(n4);
            n1.LinkTo(n4);


            return body;
        }

        public void AddBody(ChimpVPhysicsBody b)
        {
            Nodes.AddRange(b.Nodes);
        }

        public override bool OnUpdate(Vector2 pos)
        {
            base.OnUpdate(pos);

            foreach (ChimpVPhysicsNode n in Nodes)
            {
                if (n.Static) continue;
                n.Pos = n.Pos + n.Force;
                n.Pos = n.Pos + Gravity;
                n.Force = n.Force*n.Mass;
            }
            for (int j = 0; j < 100; j++)
            {
                foreach (ChimpVPhysicsNode n in Nodes)
                {
                    float xd, yd;
                    int i = 0;
                    foreach (ChimpVPhysicsNode n2 in n.Links)
                    {
                        xd = n2.Pos.X - n.Pos.X;
                        yd = n2.Pos.Y - n.Pos.Y;
                        float dis = n.LinkDist[i];
                        float ang = MathHelper.RadiansToDegrees((float) Math.Atan2(yd, xd));
                        ang += 180;
                        var dp = new Vector2(n2.Pos.X + ChimpMath.Cos(ang)*dis, n2.Pos.Y + ChimpMath.Sin(ang)*dis);
                        float sf = 0.0006f;
                        if (n.Static == false)
                        {
                            n.Force = n.Force + new Vector2((dp.X - n.Pos.X)*sf, (dp.Y - n.Pos.Y)*sf);
                        }
                        i++;
                    }
                }
                foreach (ChimpVPhysicsNode n in Nodes)
                {
                    n.Pos = n.Pos + n.Force;
                }
            }
            return false;
        }
    }
}